#ifndef _NOISE_FLT_H_
#define _NOISE_FLT_H_

#include <malloc.h>
#include <math.h>
#include <stdint.h>

template<typename analog_type>
class noise_flt {
    uint16_t idx{};

    analog_type ave() {
      uint8_t i;
      uint8_t n;
      float s = 0;

      n = depth;
      for (i = 0; i < n; i++) {
        s += data[i];
      }
      return (analog_type) (s / n);
    }

public:
    void begin(uint8_t _depth, analog_type val) {
      uint8_t i;
      data = (analog_type *) malloc(_depth * sizeof(analog_type));
      for (i = 0; i < _depth; i++) {
        data[i] = val;
      }
      depth = _depth;
      idx = 0;
    }

    ~noise_flt() { free(data); }

    int write(analog_type val) {
      data[idx++] = val;
      idx %= depth;
      return idx;
    }

    analog_type median() {
      uint8_t i;
      uint8_t n = depth;
      auto *buf = (analog_type *) malloc(depth * sizeof(analog_type)); //
      // Copy data to buf
      for (i = 0; i < n; i++) {
        buf[i] = data[i];
      }
      i = quick_median(buf, n);
      analog_type val = buf[i];
      free(buf);
      return val;
    }

    /*  限幅滤波法
      优点：
      能有效克服因偶然因素引起的脉冲干扰。
      缺点：
      无法抑制那种周期性的干扰，且平滑度差。
      A值可根据实际情况调整
      value为有效值，new_value为当前采样值
      滤波程序返回有效的实际值
       */
#define A 10
    double _value{};

    analog_type value() { return _value; }

    char filter(char new_value) {
      if ((new_value - _value > A) || (_value - new_value > A)) {
        return _value;
      } else {
        return new_value;
      }
    }

    /*  中位值滤波法
      优点：
      能有效克服因偶然因素引起的波动干扰；
      对温度、液位等变化缓慢的被测参数有良好的滤波效果。
      缺点：
      对流量，速度等快速变化的参数不宜。
      N值可根据实际情况调整
      排序采用冒泡法
       */
#define NN 11

    char middle() {
      char value_buf[NN];
      char i, j, temp;
      for (i = 0; i < NN; i++) {
        //            value_buf[i] = get_ad();
        //            delay();
      }
      for (j = 0; j < NN - 1; j++) {
        for (i = 0; i < NN - j; i++) {
          if (value_buf[i] > value_buf[i + 1]) {
            temp = value_buf[i];
            value_buf[i] = value_buf[i + 1];
            value_buf[i + 1] = temp;
          }
        }
      }
      return value_buf[(NN - 1) / 2];
    }

    /* 算数平均滤波法
     说明：
     连续取N个采样值进行算术平均运算。
     优点：
     试用于对一般具有随机干扰的信号进行滤波。
     这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动。
     缺点：
     对于测量速度较慢或要求数据计算较快的实时控制不适用
      */

    /*  递推平均滤波法
      说明：
      把连续N个采样值看成一个队列，队列长度固定为N；
      每次采样到一个新数据放入队尾，并扔掉队首的一次数据。把队列中的N各数据进行平均运算，即获得新的滤波结果
      优点：
      对周期性干扰有良好的抑制作用，平滑度高； 适用于高频振荡的系统。
      缺点：
      灵敏度低；
      对偶然出现的脉冲性干扰的抑制作用较差，不适于脉冲干扰较严重的场合
      不适合用于开关电源电路。
       */

    /*  中位值平均滤波法
      说明：
      采一组队列去掉最大值和最小值
      优点：
      融合了两种滤波的优点。对于偶然出现的脉冲性干扰，可消除有其引起的采样值偏差。
      对周期干扰有良好的抑制作用，平滑度高，适于高频振荡的系统。
      缺点：
      测量速度慢。
       */

    analog_type middle_avr() {
      int i = 0, j = 0;
      analog_type temp = 0;
      analog_type *value_buf = (analog_type *) malloc(sizeof(analog_type) * depth);
      double sum = 0;

      for (i = 0; i < depth; i++) {
        value_buf[i] = data[i];
      }

      for (j = 0; j < depth - 1; j++) {
        for (i = 0; i < depth - j; i++) {
          if (value_buf[i] > value_buf[i + 1]) {
            temp = value_buf[i];
            value_buf[i] = value_buf[i + 1];
            value_buf[i + 1] = temp;
          }
        }
      }
      for (i = 2; i < depth - 2; i++) {
        sum += value_buf[i];
      }
      _value = (sum / (depth - 4));
      free(value_buf);
      return _value;
    }

    /* 一阶滞后滤波法
     优点：
     对周期性干扰具有良好的抑制作用；
     适用于波动频率较高的场合。
     缺点：
     相位滞后，灵敏度低；
     滞后程度取决于a值大小；
     不能消除滤波频率高于采样频率1/2的干扰信号。
     为加块程序处理速度，假定基数 = 100  a = 0 ~ 100
      */

#define A 50

    float a{};

    char oneedge() {
      char new_value = 0;

      //        new_value = get_ad();
      return (1 - a) * _value + a * new_value;
    }

    /*  加权递推平均滤波法
      说明：
      是对递推平均滤波法的改进，即不同时刻的数据加以不同的权；
      通常是，越接近现时刻的数据，权取得越大。
      给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低。
      优点：
      适用于有较大纯滞后时间常数的对象，和采样周期较短的系统。
      缺点：
      对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号；
      不能迅速反应系统当前所受干扰的严重程度，滤波效果差。
      coe数组为加权系数表
       */

    // char coe[depth] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};

    char sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12;

    char avavr() {
      char i = 0;
      char value_buf[depth];
      int sum = 0;

      for (i = 0; i < depth; i++) {
        //            value_buf[i] = get_ad();
        //            delay();
      }
      for (i = 0; i < depth; i++) {
        sum += value_buf[i] * i;
      }
      return (char) (sum / sum_coe);
    }

    /*  消抖滤波法
      说明：
      设置一个滤波计数器，将每次采样值与当前有效值比较：
      如果采样值=当前有效值，则计数器清零；
              如果采样值<>当前有效值，则计数器+1，并判断计数器是否>=上限N（溢出）；
              如果计数器溢出，则将本次值替换当前有效值，并清计数器。
              优点：
              对于变化缓慢的被测参数有较好的滤波效果；
              可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。 缺点：
              对于快速变化的参数不宜；
              如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。*/

    char debound() {
      char i = 0;
      char new_value = 0;

      //        new_value = get_ad();

      while (_value != new_value) {
        i++;
        if (i > NN) {
          return new_value;
        }
        //            delay();
        //            new_value = get_ad();
      }
      return _value;
    }

    analog_type max_val(analog_type *buf, uint8_t n) {
      analog_type val = -0x7fffffff;
      for (int i = 0; i < n; i++) {
        if (val < buf[i]) {
          val = buf[i];
        }
      }
      return val;
    }

    analog_type avr_value() { return sum() / depth; }

    analog_type sum() {
      float sum = 0;
      for (int i = 0; i < depth; i++) {
        sum += data[i];
      }
      return sum;
    }

    int min_val() {
      analog_type val = 0x7fffffff;
      for (int i = 0; i < depth; i++) {
        if (val > data[i]) {
          val = data[i];
        }
      }
      return val;
    }

    double e_old, i_old;

    analog_type write_pid(analog_type new_value) {
      write(new_value);
      error = new_value - _value;
      double p_out = kp * error;

      // 计算积分控制输出
      double i_out = ki * i_old + ki * error;

      // 计算微分控制输出
      double d_out = kd * (error - e_old);

      // 计算 PID 控制器的输出
      _value += p_out + i_out + d_out;

      // 更新状态变量
      e_old = error;
      i_old = i_out;

      return _value;
    }

    // C.A.R. Hoare's Quick Median
    uint8_t quick_median(analog_type *buf, uint8_t n) {
      uint8_t l = 0;
      uint8_t r = n - 1;
      uint8_t k = n / 2;

      while (l < r) {
        analog_type x = buf[k];
        uint8_t i = l;
        uint8_t j = r;
        // Split dada
        do {
          while (buf[i] < x) {
            i++;
          }
          while (x < buf[j]) {
            j--;
          }
          if (i <= j) {
            analog_type ii = buf[i];
            buf[i] = buf[j];
            buf[j] = ii;
            i++;
            j--;
          }
        } while (i <= j);

        if (j < k) {
          l = i;
        }
        if (k < i) {
          r = j;
        }
      }
      return k;
    }

    // 高斯滤波
    void gaussian_filter(int data[], int size, int window_size) {
      // 创建一个临时数组，用于存储平滑后的结果
      int temp[size];

      // 创建一个高斯滤波器
      double filter[window_size];
      for (int i = 0; i < window_size; i++) {
        filter[i] = exp(-(i * i) / (2 * window_size * window_size));
      }

      // 遍历数据数组
      for (int i = 0; i < size; i++) {
        // 计算数据点的邻域
        int neighbors[window_size];
        for (int j = 0; j < window_size; j++) {
          if (i + j < 0 || i + j >= size) {
            neighbors[j] = 0;
          } else {
            neighbors[j] = data[i + j];
          }
        }

        // 计算数据点的邻域高斯权重
        double weights[window_size];
        for (int j = 0; j < window_size; j++) {
          weights[j] = filter[j];
        }

        // 计算数据点的平滑值
        int smooth = 0;
        for (int j = 0; j < window_size; j++) {
          smooth += neighbors[j] * weights[j];
        }

        // 将平滑后的结果存储到临时数组中
        temp[i] = smooth;
      }

      // 将临时数组中的结果复制到原始数据数组中
      for (int i = 0; i < size; i++) {
        data[i] = temp[i];
      }
    }

    // 维纳滤波
    void wiener_filter(int data[], int size, int window_size) {
      // 创建一个临时数组，用于存储平滑后的结果
      int temp[size];

      // 创建一个维纳滤波器
      double h[window_size];
      for (int i = 0; i < window_size; i++) {
        h[i] = 1 / (1 + (i * i) / (2 * window_size * window_size));
      }

      // 遍历数据数组
      for (int i = 0; i < size; i++) {
        // 计算数据点的邻域
        int neighbors[window_size];
        for (int j = 0; j < window_size; j++) {
          if (i + j < 0 || i + j >= size) {
            neighbors[j] = 0;
          } else {
            neighbors[j] = data[i + j];
          }
        }

        // 计算数据点的邻域维纳权重
        double weights[window_size];
        for (int j = 0; j < window_size; j++) {
          weights[j] = h[j];
        }

        // 计算数据点的平滑值
        int smooth = 0;
        for (int j = 0; j < window_size; j++) {
          smooth += neighbors[j] * weights[j];
        }

        // 将平滑后的结果存储到临时数组中
        temp[i] = smooth;
      }

      // 将临时数组中的结果复制到原始数据数组中
      for (int i = 0; i < size; i++) {
        data[i] = temp[i];
      }
    }

    analog_type *data;
    volatile uint16_t depth{};
    double error2, error1, error;
    analog_type last_value;
    double kp = 0.3, ki = 0.3, kd = 0;
};

#endif /* _NOISE_FLT_H_ */
